/**
 * @file           : physicalVector.h
 * @author         : Star_Plucking
 * @brief          : 物理向量运算库，带有单位的那种
 * @date           : 25-4-6 下午12:05
 * @lastEditor     :
 * @lastEditTime   : 25-4-6 下午12:05
 */
#pragma once
#include <algorithm/math.h>
#include <units.h>

namespace os::physics {
template <typename Unit>
struct Vector2;

// 通用三维向量模板
template <typename Unit>
struct Vector3 {
    Unit x, y, z;

    constexpr Vector3(Unit x, Unit y, Unit z) : x(x), y(y), z(z) {
    }

    // 允许复制定义
    constexpr Vector3(Vector3 const&) = default;

    // 重载等于号
    constexpr Vector3& operator=(Vector3 const& other) {
        x = other.x;
        y = other.y;
        z = other.z;
        return *this;
    }

    constexpr Vector3 operator+(Vector3 const& other) const { return {x + other.x, y + other.y, z + other.z}; }
    constexpr Vector3 operator-(Vector3 const& other) const { return {x - other.x, y - other.y, z - other.z}; }
    constexpr Vector3 operator*(float scalar) const { return {x * scalar, y * scalar, z * scalar}; }
    constexpr Vector3 operator/(float scalar) const { return {x / scalar, y / scalar, z / scalar}; }

    // 带单位的除法
    template <class Unit1>
    constexpr auto operator/(Unit1 const& num) {
        using ResultUnit = decltype(Unit{} / Unit1{}); // 自动计算单位
        float const result[3] = {x() / num(), y() / num(), z() / num()};
        return Vector3<ResultUnit>(units::make_unit<ResultUnit>(result[0]), units::make_unit<ResultUnit>(result[1]),
                                   units::make_unit<ResultUnit>(result[2]));
    }

    // 带单位的乘法
    template <class Unit1>
    constexpr auto operator*(Unit1 const& num) {
        using ResultUnit = decltype(Unit{} / Unit1{}); // 自动计算单位
        float const result[3] = {x() * num(), y() * num(), z() * num()};
        return Vector3<ResultUnit>(units::make_unit<ResultUnit>(result[0]), units::make_unit<ResultUnit>(result[1]),
                                   units::make_unit<ResultUnit>(result[2]));
    }

    /**
     * @brief 带单位的点乘
     * @param other 右侧点乘向量
     * @return 点乘结果（带有单位
     */
    constexpr auto dot(Vector3 const& other) const { return x * other.x + y * other.y + z * other.z; }

    /**
     * @brief 矢量叉乘（返回结果为新向量）
     * @param other 右侧叉乘向量
     * @return 叉乘结果（原单位）
     */
    constexpr Vector3 cross(Vector3 const& other) const {
        return {(y * other.z - z * other.y), (z * other.x - x * other.z), (x * other.y - y * other.x)};
    }

    /**
     * @brief 计算模长
     * @return 矢量模长（带有单位）
     */
    constexpr auto magnitude() const { return units::math::sqrt(x * x + y * y + z * z); }

    /**
     * @brief 计算向量幅角 (X-Y平面)
     * @return 矢量幅角（默认弧度单位）
     */
    constexpr units::angle::radian_t angle_yx() const { return units::math::atan2(y, x); }

    // 归一化向量
    constexpr Vector3 normalize() const {
        auto mag = magnitude();
        return {x / mag, y / mag, z / mag};
    }

    // 矢量投影
    constexpr Vector3 project_onto(Vector3 const& other) const {
        auto unit_other = other.normalize();
        auto projection_magnitude = this->dot(unit_other);
        return unit_other * projection_magnitude;
    }

    /**
     * @brief 转换为二维维向量（直接舍弃Z向量）
     * @return 转换后的二维向量
     */
    constexpr Vector2<Unit> to2D() const { return {x, y}; }

    // 友元函数：标量乘法（左侧）
    friend constexpr Vector3 operator*(double scalar, Vector3 const& vec) { return vec * scalar; }
};

struct RotationMatrix {
    units::angle::radian_t alpha, beta, gamma;

    RotationMatrix(units::angle::radian_t alpha, units::angle::radian_t beta, units::angle::radian_t gamma)
        : alpha(alpha), beta(beta), gamma(gamma) {
    }

    // 重载乘法，可以旋转向量Vector3
    template <typename Unit>
    constexpr auto operator*(Vector3<Unit> const& input) {
        float const cos_alpha = os::math::cos(alpha);
        float const sin_alpha = os::math::sin(alpha);
        float const cos_beta = os::math::cos(beta);
        float const sin_beta = os::math::sin(beta);
        float const cos_gamma = os::math::cos(gamma);
        float const sin_gamma = os::math::sin(gamma);
        return Vector3<Unit>(
            input.x * (cos_beta * cos_gamma) + input.y * (cos_alpha * sin_gamma + sin_alpha * sin_beta * cos_gamma) +
            input.z * (sin_alpha * sin_gamma - cos_alpha * sin_beta * cos_gamma),
            input.x * (-cos_beta * sin_gamma) + input.y * (cos_alpha * cos_gamma - sin_alpha * sin_beta * sin_gamma) +
            input.z * (sin_alpha * cos_gamma + cos_alpha * sin_beta * sin_gamma),
            input.x * (sin_beta) + input.y * (-sin_alpha * cos_beta) + input.z * (cos_alpha * cos_beta));
    }
};

/**
 * @brief 自动推导单位的叉乘函数
 * @tparam Unit1 左向量单位
 * @tparam Unit2 右向量单位
 * @param vec1 左向量
 * @param vec2 右向量
 * @return 结果（自动推导单位）
 */
template <typename Unit1, typename Unit2>
constexpr auto cross(Vector3<Unit1> const& vec1, Vector3<Unit2> const& vec2) {
    using ResultUnit = decltype(Unit1{} * Unit2{}); // 自动计算单位
    float result[3] = {(vec1.y() * vec2.z() - vec1.z() * vec2.y()), (vec1.z() * vec2.x() - vec1.x() * vec2.z()),
                       (vec1.x() * vec2.y() - vec1.y()) * vec2.x()};
    return Vector3<ResultUnit>(units::make_unit<ResultUnit>(result[0]), units::make_unit<ResultUnit>(result[1]),
                               units::make_unit<ResultUnit>(result[2]));
}

// 常见物理向量类型
using LengthVector = Vector3<units::length::meter_t>;
using VelocityVector = Vector3<units::velocity::meters_per_second_t>;
using ForceVector = Vector3<units::force::newton_t>;
using TorqueVector = Vector3<units::torque::newton_meter_t>;

// 通用二维向量模板
template <typename Unit>
struct Vector2 {
    Unit x, y;

    constexpr Vector2(Unit x, Unit y) : x(x), y(y) {
    }

    constexpr Vector2(Unit rho, units::angle::radian_t theta)
        : x(rho * units::math::cos(theta)), y(rho * units::math::sin(theta)) {
    }

    constexpr Vector2 operator+(Vector2 const& other) const { return {x + other.x, y + other.y}; }
    constexpr Vector2 operator-(Vector2 const& other) const { return {x - other.x, y - other.y}; }
    constexpr Vector2 operator*(float scalar) const { return {x * scalar, y * scalar}; }
    constexpr Vector2 operator/(float scalar) const { return {x / scalar, y / scalar}; }

    /**
     * @brief 带单位的点乘
     * @param other 右侧点乘向量
     * @return 点乘结果（带有代为
     */
    constexpr auto dot(Vector2 const& other) const { return x * other.x + y * other.y; }

    /**
     * @brief 计算模长
     * @return 矢量模长（带有单位）
     */
    constexpr auto magnitude() const { return units::math::sqrt(x * x + y * y); }

    /**
     * @brief 计算向量幅角 (X-Y平面)
     * @return 矢量幅角（默认弧度单位）
     */
    constexpr units::angle::radian_t angle() const { return units::math::atan2(y, x); }

    // 归一化向量
    constexpr Vector2 normalize() const {
        auto mag = magnitude();
        return {x / mag, y / mag};
    }

    // 矢量投影
    constexpr Vector2 project_onto(Vector2 const& other) const {
        auto unit_other = other.normalize();
        auto projection_magnitude = this->dot(unit_other);
        return unit_other * projection_magnitude;
    }

    // 友元函数：标量乘法（左侧）
    friend constexpr Vector2 operator*(double scalar, Vector2 const& vec) { return vec * scalar; }

    // 转换为三维向量（z=0）
    constexpr Vector3<Unit> to3D() const { return {x, y, Unit(0)}; }
};

// 常见物理二维向量类型
using LengthVector2 = Vector2<units::length::meter_t>;
using VelocityVector2 = Vector2<units::velocity::meters_per_second_t>;
using ForceVector2 = Vector2<units::force::newton_t>;
} // namespace os::physics